import sensor, image, time,math
from machine import UART
from fpioa_manager import fm


enable_lens_corr = False # turn on for straighter lines...打开以获得更直的线条…
import sensor, image, time
from machine import UART,Timer
from fpioa_manager import fm

import machine
import sensor
import lcd
import time
import image
import sys
import utime
from Maix import GPIO
from Maix import freq
from fpioa_manager import fm
from board import board_info
from machine import UART
from machine import Timer
import math




#选择两个引脚，K210的引脚配置非常灵活，具体可以参考官方手册
fm.register(6, fm.fpioa.UART1_TX, force=True)
fm.register(7, fm.fpioa.UART1_RX, force=True)
#串口初始化
uart = UART(UART.UART1, 115200, 8, 1, 0, timeout=1000, read_buf_len=4096)
uart.init(115200, bits=8, parity=None, stop=1, timeout_char=1000)
#设置帧头帧尾
u_start=bytearray([0xb3,0x01])
u_over=bytearray([0x0d,0x0a])


# RGB初始化
fm.register(board_info.LED_R,fm.fpioa.GPIO0)
led_r=GPIO(GPIO.GPIO0,GPIO.OUT)
fm.register(board_info.LED_B,fm.fpioa.GPIO1)
led_b=GPIO(GPIO.GPIO1,GPIO.OUT)
fm.register(board_info.LED_G,fm.fpioa.GPIO2)
led_g=GPIO(GPIO.GPIO2,GPIO.OUT)
led_g.value(1)
led_b.value(1)
led_r.value(1)


# 全局变量定义
led_cnt = 0  # LED闪烁计数位
status = 0  # LED状态位
Difference = 0  # 和中心的差值


min_degree0 = 35
max_degree0 = 145


min_degree90 = 75
max_degree90 = 105

flag_heng=0
flag_lie=0
# 所有线段都有 `x1()`, `y1()`, `x2()`, and `y2()` 方法来获得他们的终点
# 一个 `line()` 方法来获得所有上述的四个元组值，可用于 `draw_line()`.
degree_0=[0,0,0,0,0]
rho_0=[0,0,0,0,0]

degree0_num=0
rho0_num=0

degree_90=[0,0,0,0,0]#窗口为5的滑动滤波
rho_90=[0,0,0,0,0]

degree90_num=0
rho90_num=0

encoder_filter_index = 0



sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 2000)
#sensor.set_auto_gain(False) # must be turned off for color tracking
sensor.set_auto_whitebal(False) # must be turned off for color tracking
#关闭白平衡
clock = time.clock()




def sending_data(data0,data1,data2,data3,data4):
    global u_start
    global u_over
    global uart
    row_data=[data0,data1,data2,data3,data4]
    print(row_data)

    uart_buf = bytearray(row_data)#转换格式
    uart.write(u_start)
    uart.write(uart_buf)
    uart.write(u_over)

# 定时器中断回调函数
def on_timer(timer):

    # print("timer")
    global led_cnt
    global status

    # 红色LED闪烁
    led_cnt+=1
    if led_cnt >= 50:
        led_r.value(status)
        led_cnt = 0
        status = 0 if (status == 1) else 1

# 初始化定时器中断  TIMER1       通道0           中断模式           TIMER周期5ms    周期单位ms         回调函数   希望传给回调函数的参数  立即开启定时器   中断优先级  分频
tim = Timer(Timer.TIMER1, Timer.CHANNEL0, mode=Timer.MODE_PERIODIC, period=5, unit=Timer.UNIT_MS, callback=on_timer, arg=on_timer, start=False, priority=1, div=0)
#print("period:",tim.period())  # 打印定时器参数



roi2=(101,2,1354,239)



tim.start()  # 开启定时器

while(True):
    times=0
    clock.tick() # Track elapsed milliseconds between snapshots().


    data=[0,0,0,0,0]
    img = sensor.snapshot()
    if enable_lens_corr: img.lens_corr(1.8) # for 2.8mm lens...

    # `threshold` controls how many lines in the image are found. Only lines with
    # edge difference magnitude sums greater than `threshold` are detected...

    # `threshold`控制从霍夫变换中监测到的直线。只返回大于或等于阈值的
    # 直线。应用程序的阈值正确值取决于图像。注意：一条直线的大小是组成
    # 直线所有索贝尔滤波像素大小的总和。

    # `theta_margin`和`rho_margin`控件合并相似的直线。如果两直线的
    # theta和ρ值差异小于边际，则它们合并。

    min_l_rho=800
    row_data=[0x00,0x00,0x00,0x00,0x00]
    flag_getlines=0
    for l in img.find_lines(ROI=roi2,threshold = 2000, theta_margin = 50, rho_margin = 50):
        if (max_degree0 <= l.theta()) or (l.theta() <= min_degree0):#判断平行线 巡线
            if(l.rho()<=min_l_rho):
                get_l=l
                img.draw_line(get_l.line(), color = (255, 0, 0))
                flag_getlines=1

        if (min_degree90 <= l.theta()) and (l.theta() <= max_degree90):#判断垂直线 特征点
            img.draw_line(l.line(), color = (0, 0, 255))
            row_data[0]=0x01
        else:
            row_data[0]=0x00

    if(flag_getlines):
        encoder_filter_index+=1
        if(get_l.theta()<=min_degree0):
            degree_0[encoder_filter_index%5]=180+get_l.theta()
            rho_0[encoder_filter_index%5]=get_l.rho()
        else:
            degree_0[encoder_filter_index%5]=get_l.theta()
            rho_0[encoder_filter_index%5]=-get_l.rho()
        sum = 0
        for k in degree_0:
            sum =sum+ k
        degree0_num= sum/5
        sum = 0

        for k in rho_0:
            sum =sum+ k
        rho0_num= sum/5
        sum = 0




#        print("degree0_num:%f" %(degree0_num))  # 打印和中心位置的偏差



 #       print("rho0_num:%f" %(rho0_num))  # 打印和中心位置的偏差

        row_data[1]=int(degree0_num)
        row_data[3]=int(rho0_num)

        uart_buf =    bytearray(row_data)#转换格式
        uart.write(u_start)
        uart.write(uart_buf)
        uart.write(u_over)


        print(row_data)

    #print("degree90_num:%f" %(degree90_num))  # 打印和中心位置的偏差
    #print("rho90_num:%f" %(rho90_num))  # 打印和中心位置的偏差


    #print("FPS %f" % clock.fps())








#********************串口***********************#

    #print(row_data)
    #uart_buf = bytearray(row_data)#转换格式
    #uart.write(u_start)
    #uart.write(uart_buf)
    #uart.write(u_over)





#if text:
#    print(text.decode('utf-8')) #REPL打印
    #uart.write('I got'+text.decode('utf-8')) #数据回传
#print("FPS %f" % clock.fps())



# About negative rho values:
# 关于负rho值:
#
# A [theta+0:-rho] tuple is the same as [theta+180:+rho].
# A [theta+0:-rho]元组与[theta+180:+rho]相同。
